#! /usr/bin/env python3
# -*- coding: UTF-8 -*-
# 上面两行不可省略，第一行是：告诉操作系统执行这个脚本的时候，调用 /usr/bin 下的 python 解释器。第二行是：定义编码格式 "UTF-8-" 支持中文

from actionlib.action_client import GoalManager
import rospy
import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from std_msgs.msg import String

def doMsg(msg):
    client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
    client.wait_for_server()
    # 定义四个发送目标点的对象
    goal0 = MoveBaseGoal()
    # 初始化四个目标点在 map 坐标系下的坐标,数据来源于《采集的目标点.docx》
    message = msg.data
    rospy.loginfo("%s received!"%message)
    if(message == "A"):
        # etting goal: Frame:map, Position(-0.674, -0.176, 0.000), Orientation(0.000, 0.000, -0.801, 0.599) = Angle: -1.858
        goal0.target_pose.pose.position.x = 1.185
        goal0.target_pose.pose.position.y = -4.334
        goal0.target_pose.pose.orientation.z = -0.416
        goal0.target_pose.pose.orientation.w = 0.909
    if(message == "B"):
        goal0.target_pose.pose.position.x = 2.377
        goal0.target_pose.pose.position.y = 9.428
        goal0.target_pose.pose.orientation.z = 0.664
        goal0.target_pose.pose.orientation.w = 0.748
    if(message == "C"):
        goal0.target_pose.pose.position.x = -3.169
        goal0.target_pose.pose.position.y = 5.355
        goal0.target_pose.pose.orientation.z = -0.785
        goal0.target_pose.pose.orientation.w = 0.620
    if(message == "D"):
        goal0.target_pose.pose.position.x = 1.84500110149
        goal0.target_pose.pose.position.y = -0.883078575134
        goal0.target_pose.pose.orientation.z = -0.635
        goal0.target_pose.pose.orientation.w = 0.773

    goal_lists = [goal0]  # 采用 python 中的列表方式，替代实现C/C++ 中的数组概念
    goal_lists[0].target_pose.header.frame_id = "map"
    goal_lists[0].target_pose.header.stamp = rospy.Time.now()
    client.send_goal(goal_lists[0])
    str_log = "Send NO. %s Goal !!!" % str(0)
    rospy.loginfo(str_log)


    wait = client.wait_for_result(rospy.Duration.from_sec(300.0))  # 发送完毕目标点之后，根据action 的机制，等待反馈执行的状态，等待时长是：30 s.
    if not wait:
        str_log = "The NO. %s Goal Planning Failed for some reasons" % str(0)
        rospy.loginfo(str_log)
    else:
        str_log = "The NO. %s Goal achieved success !!!" % str(0)
        rospy.loginfo(str_log)
    return "Mission Finished."

if __name__ == '__main__':
    rospy.init_node("listener_p")
    rospy.loginfo("start listening...")
    #3.实例化 订阅者 对象
    sub = rospy.Subscriber("/dingzhen",String,doMsg,queue_size=10)
    rospy.spin()